import time

from robomaster import conn
from multi_robomaster import multi_robot
from robomaster import led, blaster




if __name__ == '__main__':
    # robomaster.config.LOCAL_IP_STR = "192.168.1.111"
    robots_sn_list = ['3JKDH2T00159G8','3JKCJC400302GS','3JKCJC400301ZP', '3JKCJC400301UD','3JKCJC400301W0', '3JKCJC400300Y9']

    multi_robots = multi_robot.MultiEP()
    multi_robots.initialize()

    number = multi_robots.number_id_by_sn([0, robots_sn_list[0]], [1, robots_sn_list[1]], [2, robots_sn_list[2]],
                                          [3, robots_sn_list[3]], [4, robots_sn_list[4]], [5, robots_sn_list[5]])
    print("The number of robot is: {0}".format(number))
    robot_group_all = multi_robots.build_group([0, 1, 2, 3, 4, 5])
    robot_group_ep_gimbal = multi_robots.build_group([0, 2, 4])
    robot_group_ep_arm = multi_robots.build_group([1, 3, 5])